#ifndef MY_INC_PHY_VSG_H
#define MY_INC_PHY_VSG_H

#include "MY_INC/PHY/PHY_Cordic.h" // CORDIC库
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#define PI              3.14159265358979323846f
#define VSG_BUFFER_SIZE 1024     // 环形缓冲区大小(2的幂次)
#define VSG_SAMPLE_RATE 50000.0f // 50kHz采样率

typedef struct
{
    float TOGI_k; // TOGI增益
    float TOGI_T; // TOGI时间常数

    float Integral1_Yn;   // 积分器1
    float Integral1_Xn;   // 积分器1
    float Integral1_Xn_1; // 积分器1

    float Integral2_Yn;   // 积分器1
    float Integral2_Xn;   // 积分器1
    float Integral2_Xn_1; // 积分器1

    float Integral3_Yn;   // 积分器1
    float Integral3_Xn;   // 积分器1
    float Integral3_Xn_1; // 积分器1

    float U1; // 输出1
    float U2; // 输出2
    float U3; // 输出3

    float Alpha; // Alpha电压
    float Beta;  // Beta电压

    float D; // D电压
    float Q; // Q电压
} VSGTOGI;
// 辅助结构体，用于封装积分器的状态
typedef struct
{
    float Xn;      // 当前输入
    float Xn_1;    // 上一时刻输入
    float Yn;      // 当前输出（积分结果）
    float ts_half; // 采样时间的一半
} VSGIntegral;
typedef struct
{
    // 移相专用
    float    buffer[VSG_BUFFER_SIZE]; // 采样数据环形缓冲区
    uint16_t write_index;             // 缓冲区写指针
    // RMS专用
    float Vrms; // 输出：信号的均方根值（RMS，Vrms）
    float Vacc_rms;
    // 通用检测变量
    uint16_t nsamplesMin; // 样本数最小限制（用于过滤异常值，nsamplesMin）
    uint16_t nsamplesMax; // 样本数最大限制（用于过滤异常值，nsamplesMax）
    float    curr_sample_norm;
    bool     prev_sign;
    bool     curr_sign;
    uint16_t nsamples;
    float    inv_nsamples;
    uint16_t buffer_size;
    float    SigFreq;
    float    SampleFreq;
} VSGSignalAnalysisr;
typedef struct
{
    float A0;         // 系数
    float B0;         // 系数
    float ERROR;      // 当前误差
    float ERROR_Last; // 前一次误差
    float Out;        // 误差累加
    float output_min; // 输出最小值
    float output_max; // 输出最大值
} VSGPI;
typedef struct
{
    // PI 控制器参数
    float Coeff_B0; // 比例增益
    float Coeff_B1; // 积分增益
    float Yn;       // 上一次输出
    float Xn;       // 上一次输出
    float Xn_1;     // 上一次输出
    // 相位和频率
    float Theta; // 相位
    // 状态变量
    float Integral_Xn;   // 上一次输入
    float Integral_Xn_1; // 上一次输入
    float Integral_Yn;   // 上一次输出
    float ts_half;       // 0.5 * ts，用于计算
} VSGPLL;
// 主结构体，用于封装VSG_P函数的所有状态
typedef struct
{

    float Pe;    // 功率参考值和实际值
    float D;     // 阻尼系数
    float J;     // 惯量
    float W;     // 内部角频率
    float Inv_J; // 惯量
    float ts;    // 采样时间
    float wt;    // 相角

    float Qe;       // 功率参考值和实际值
    float Un;       // 无功下垂系数
    float VSG_Q_Kq; // 无功下垂系数
    float VSG_Q_Kp; // 无功幅值比例系数
    float E0;       // 最终输出的幅值
    float E;        // 最终合成的电动势

    float Sine;   // 正弦值
    float Cosine; // 余弦值

    float dU;     // 预同步电压差值
    float dTheta; // 预同步相位差值

    VSGIntegral        Integral1;          // 积分器1的状态
    VSGIntegral        Integral2;          // 积分器2的状态
    VSGIntegral        Integral3;          // 积分器3的状态
    VSGTOGI            TOGI_V;             // 电压的 TOGI变换
    VSGTOGI            TOGI_I;             // 电流的 TOGI变换
    VSGSignalAnalysisr SignalAnalysisr_VG; // 信号分析器状态
    VSGSignalAnalysisr SignalAnalysisr_E;  // 信号分析器状态
    VSGPI              PI_U;               // 电压预同步的 PI 控制器
    VSGPI              PI_theta;           // 相位预同步的 PI 控制器
    VSGPLL             PLL;                // 锁相环的 PI 控制器
} VSG_Typedef;
static inline void  VSG_PLLCalculate_Inline(VSGPLL *PLL, float Actual);
static inline float VSG_PIControl_Inline(VSGPI *pi, float target, float actual);
static inline void  VSG_SignalAnalysisr_Inline(VSGSignalAnalysisr *v, float adc_sample);
static inline void  VSG_TOGItoDQ_Inline(VSGTOGI *transform, float Input, float W, float *Sin_Data, float *Cos_Data);
static inline void  VSG_TOGItoAphlaBeta_Inline(VSGTOGI *transform, float Input, float W);

static inline void VSG_PLLCalculate_Inline(VSGPLL *PLL, float Actual)
{
    /*===================== PI控制器计算 =====================*/
    PLL->Xn = Actual * 0.005f + 0.995f * PLL->Xn; // 低通滤波器
    PLL->Yn += PLL->Coeff_B0 * PLL->Xn + PLL->Coeff_B1 * PLL->Xn_1;
    PLL->Xn_1 = PLL->Xn; // 更新上一次输入
    /*===================== 计算积分输出 =====================*/
    // 计算输出
    PLL->Integral_Xn = PLL->Yn + (2.0f * 50.0f * PI);
    // % 计算积分输出
    PLL->Integral_Yn += PLL->ts_half * (PLL->Integral_Xn + PLL->Integral_Xn_1);
    if (PLL->Integral_Yn > (2.0f * PI))
        PLL->Integral_Yn -= (2.0f * PI);
    else if (PLL->Integral_Yn < 0.0f)
        PLL->Integral_Yn += (2.0f * PI);
    // 更新状态
    PLL->Theta = PLL->Integral_Yn;
}
static inline float VSG_PIControl_Inline(VSGPI *pi, float target, float actual)
{
    // 计算当前误差
    pi->ERROR = target - actual;
    pi->Out += (pi->A0 * pi->ERROR) + (pi->B0 * pi->ERROR_Last);
    pi->ERROR_Last = pi->ERROR;
    // 限制输出
    if (pi->Out > pi->output_max)
        pi->Out = pi->output_max;
    else if (pi->Out < pi->output_min)
        pi->Out = pi->output_min;
    return pi->Out;
}
static inline void VSG_SignalAnalysisr_Inline(VSGSignalAnalysisr *v, float adc_sample)
{
    v->curr_sign = (bool)(adc_sample > 0.0f); // 过零检测
    v->nsamples++;
    float sample = adc_sample * adc_sample; // 计算当前采样值的平方
    v->Vacc_rms += (sample) - (v->buffer[v->write_index]);
    v->buffer[v->write_index] = sample;                       // 存储新样本到环形缓冲区
    v->write_index = (v->write_index + 1) % (v->buffer_size); // 更新写指针(环形缓冲区)
    if ((!v->prev_sign) & (v->curr_sign))
    {
        if (((v->nsamplesMin < v->nsamples) & (v->nsamples < v->nsamplesMax)))
        {
            v->inv_nsamples = (1.0f) / (v->nsamples);
            v->buffer_size = v->nsamples;
            v->SigFreq = (v->SampleFreq * v->inv_nsamples); // 计算信号频率（SigFreq）
        }
        else
        {
            v->inv_nsamples = (1.0f) / ((float)VSG_SAMPLE_RATE / 50.0f);
            v->buffer_size = VSG_BUFFER_SIZE;
            v->SigFreq = 50.0f;
        }
        v->write_index = v->nsamples = 0; // 清空样本数
    }
    v->prev_sign = v->curr_sign;                                                  // 更新上一个符号
    v->Vrms = sqrtf(v->Vacc_rms * v->inv_nsamples) * 0.0005f + 0.9995f * v->Vrms; // d返回 RMS 值
}
static inline void VSG_TOGItoDQ_Inline(VSGTOGI *transform, float Input, float W, float *Sin_Data, float *Cos_Data)
{
    float temp = (Input - transform->U1) * transform->TOGI_k;
    transform->Integral1_Xn = (temp - transform->U3) * W;
    transform->Integral1_Yn += transform->TOGI_T * (transform->Integral1_Xn + transform->Integral1_Xn_1); // 积分
    transform->Integral1_Xn_1 = transform->Integral1_Xn;                                                  // 移位
    transform->U3 = transform->Integral1_Yn;

    transform->Integral2_Xn = (temp - transform->U2) * W;
    transform->Integral2_Yn += transform->TOGI_T * (transform->Integral2_Xn + transform->Integral2_Xn_1); // 积分
    transform->Integral2_Xn_1 = transform->Integral2_Xn;                                                  // 移位
    transform->U1 = transform->Integral2_Yn;

    transform->Integral3_Xn = (transform->U1 * W);
    transform->Integral3_Yn += transform->TOGI_T * (transform->Integral3_Xn + transform->Integral3_Xn_1); // 积分
    transform->Integral3_Xn_1 = transform->Integral3_Xn;                                                  // 移位
    transform->U2 = transform->Integral3_Yn;
    transform->Alpha = transform->U1;
    transform->Beta = transform->U2 - transform->U3;

    transform->D = transform->Alpha * (*Sin_Data) - transform->Beta * (*Cos_Data);
    transform->Q = transform->Alpha * (*Cos_Data) + transform->Beta * (*Sin_Data);
}

static inline void VSG_TOGItoAphlaBeta_Inline(VSGTOGI *transform, float Input, float W)
{
    float temp = (Input - transform->U1) * transform->TOGI_k;
    transform->Integral1_Xn = (temp - transform->U3) * W;
    transform->U3 = transform->Integral1_Yn =
        transform->Integral1_Yn + transform->TOGI_T * (transform->Integral1_Xn + transform->Integral1_Xn_1); // 积分
    transform->Integral1_Xn_1 = transform->Integral1_Xn;                                                     // 移位

    transform->Integral2_Xn = (temp - transform->U2) * W;
    transform->U1 = transform->Integral2_Yn =
        transform->Integral2_Yn + transform->TOGI_T * (transform->Integral2_Xn + transform->Integral2_Xn_1); // 积分
    transform->Integral2_Xn_1 = transform->Integral2_Xn;                                                     // 移位

    transform->Integral3_Xn = (transform->U1 * W);
    transform->U2 = transform->Integral3_Yn =
        transform->Integral3_Yn + transform->TOGI_T * (transform->Integral3_Xn + transform->Integral3_Xn_1); // 积分
    transform->Integral3_Xn_1 = transform->Integral3_Xn;                                                     // 移位
    transform->Alpha = transform->U1;
    transform->Beta = transform->U2 - transform->U3;
}
static inline void VSG_Integral_Inline(VSGIntegral *Integral, float Xn)
{
    Integral->Xn = Xn;
    Integral->Yn += Integral->ts_half * (Integral->Xn + Integral->Xn_1); // 梯形积分
    Integral->Xn_1 = Integral->Xn;
}
/*带预同步单元 电压相位同步*/
static inline void VSG_Control_Inline(VSG_Typedef *control, float Pref, float Qref, float Vo, float Io)
{
    /*分解出DQ分量*/
    VSG_TOGItoDQ_Inline(&control->TOGI_V, Vo, control->W, &control->Sine,
                        &control->Cosine);                        // 将电压从TOGI变换到DQ
    VSG_TOGItoAphlaBeta_Inline(&control->TOGI_I, Io, control->W); // 将电流从TOGI变换到Alpha-Beta
    /*预同步锁相环*/
    VSG_PLLCalculate_Inline(&control->PLL, control->TOGI_V.Q);                                   // 锁相环计算
    control->dTheta = VSG_PIControl_Inline(&control->PI_theta, control->PLL.Theta, control->wt); // 相位预同步 PI 控制器

    float Pe = 0.5f * (control->TOGI_V.Alpha * control->TOGI_I.Alpha + control->TOGI_V.Beta * control->TOGI_I.Beta);
    float Qe = 0.5f * (control->TOGI_V.Beta * control->TOGI_I.Alpha - control->TOGI_V.Alpha * control->TOGI_I.Beta);
    // 更新有功无功实际值
    control->Pe = Pe * 0.005f + control->Pe * 0.995f; // 低通滤波
    control->Qe = Qe * 0.005f + control->Qe * 0.995f; // 低通滤波
    /*有功调频环路 */
    // 积分器1的计算 (角频率w的导数积分)
    VSG_Integral_Inline(&control->Integral1,
                        ((Pref - control->Pe) / control->W - (control->D * control->Integral1.Yn)) *
                            control->Inv_J);                         // 第一次积分
                                                                     // 更新角频率W0
    control->W = control->Integral1.Yn + (float)(2.0f * 50.0f * PI); // 100 * PI
    // 积分器2的计算 (角频率w的积分得到角度wt)
    VSG_Integral_Inline(&control->Integral2, control->W); // 第一次积分

    if (control->Integral2.Yn >= 2.0f * PI) // 角度归一化到 [0, 2*PI) 范围
        control->Integral2.Yn -= 2.0f * PI;
    else if (control->Integral2.Yn < 0.0f)
        control->Integral2.Yn += 2.0f * PI;

    control->wt = control->Integral2.Yn + control->dTheta; // 更新角度wt

    /*电压预同步 */
    VSG_SignalAnalysisr_Inline(&control->SignalAnalysisr_VG, Vo);        // 信号分析器处理电压样本
    VSG_SignalAnalysisr_Inline(&control->SignalAnalysisr_E, control->E); // 信号分析器处理输出电压参考值样本
    control->dU =
        VSG_PIControl_Inline(&control->PI_U, control->SignalAnalysisr_VG.Vrms, control->E0); // 电压预同步 PI 控制器
    /*无功调压环路 */
    VSG_Integral_Inline(&control->Integral3, (control->VSG_Q_Kp * (control->Un - control->SignalAnalysisr_VG.Vrms)) +
                                                 Qref - control->Qe); // 第一次积分
    control->E0 = control->Integral3.Yn * control->VSG_Q_Kq;          // 计算最终的电压参考值
    /*合成最终的电压参考值*/
    Cordic_SinCos_Inline(control->wt, &control->Sine, &control->Cosine); // 计算正弦和余弦值
    control->E = control->E0 * control->Sine + control->dU;
}
#endif // VSG_TYPES_H